package org.femto.math.vis;

import javax.media.opengl.*;
import javax.media.opengl.glu.*;
import com.sun.opengl.util.*;

import org.femto.math.Util;
import org.femto.math.linalg.*;
import org.femto.compmod.vis.*;

public class ArcBall implements Manipulator {

    private enum STATE {
        INACTIVE,
        TRACKBALL,
        CIRCLE_X,
        CIRCLE_Y,
        CIRCLE_Z,
        CIRCLE_PERIM
    }

    private final Vec3[] stateColor = {
        new Vec3(0,0,0),
        new Vec3(0,0,0),
        new Vec3(1,0,0),
        new Vec3(0,1,0),
        new Vec3(0,0,1),
        new Vec3(1,1,0)
    };

    private final Vec3[] basis = {
        new Vec3(0,0,0),
        new Vec3(0,0,0),
        new Vec3(1,0,0),
        new Vec3(0,1,0),
        new Vec3(0,0,1),
        new Vec3(0,0,0)
    };

    private final Vec3[] basisPerpBasis1 = {
        new Vec3(0,0,0),
        new Vec3(0,0,0),
        new Vec3(0,1,0),
        new Vec3(1,0,0),
        new Vec3(1,0,0),
        new Vec3(0,0,0)
    };

    private final Vec3[] basisPerpBasis2 = {
        new Vec3(0,0,0),
        new Vec3(0,0,0),
        new Vec3(0,0,1),
        new Vec3(0,0,1),
        new Vec3(0,1,0),
        new Vec3(0,0,0)
    };

    private STATE state = STATE.INACTIVE;
    private boolean mouseOver = false;
    private boolean mouseOverPerim = false;
    private boolean mouseOverX = false;
    private boolean mouseOverY = false;
    private boolean mouseOverZ = false;

    private double radius;
    private double radius2;

    private Vec3 u0 = new Vec3();
    private Vec3 u1 = new Vec3();
    private Vec3 v = new Vec3();
    private Quat q = new Quat();
    
    private Vec3 pivot = new Vec3();
    private Vec3 cursorProj = new Vec3();
    private Vec3 e1 = new Vec3();
    private Vec3 e2 = new Vec3();
    private double alpha0 = 0;

    private double alpha1 = 0;

    // Initial rotation of the ball
    private Matrix4x4 R = new Matrix4x4();
    // Differential rotation done by the user
    private Matrix4x4 dR = new Matrix4x4();

    private Matrix4x4 projection = new Matrix4x4();

    private double eps;
    private int segments;
    private final float lineWidth = 1.0f;
    private final float lineWidthHighlight = 2.5f;
    private final double outerCircleRadiusCoeff = 1.2;

    public ArcBall() {
        radius = 0.8;
        radius2 = radius*radius;
        segments = (int) (radius*100);
        eps = radius*Math.PI*2.0*0.03;
    }

    public void move(Vec3 p) {
        mouseOver = p.norm() <= radius ? true : false;
        mouseOverPerim = p.norm() > radius && p.norm() <= radius*outerCircleRadiusCoeff ? true : false;
        mouseOverX = mouseOverY = mouseOverZ = false;

        if(mouseOver) {
            // Determine if the user is pointing at one of the great circles.

            Vec3 u = Util.cartesian2spherical(R.transpose().mul(projectOntoSphere(p)));

            if((Math.abs(u.v[2] - Math.PI/2) < eps || Math.abs(u.v[2] + Math.PI/2) < eps) &&
               Math.abs(u.v[1]) < Math.PI && Math.abs(u.v[1]) >= 0) {
               mouseOverX = true;
            }

            // theta pi or 0 while phi is in [0,pi]
            if((Math.abs(u.v[2] - Math.PI) < eps || Math.abs(u.v[2]) < eps) &&
               Math.abs(u.v[1]) <= Math.PI && Math.abs(u.v[1]) >= 0)  {
               mouseOverY = true;
            }

            if(Math.abs(u.v[1] - Math.PI/2) < eps) {
                mouseOverZ = true;
            }
        }
    }

    public void beginDrag(Vec3 p) {
        if(mouseOverX == true) {
            state = STATE.CIRCLE_X;
        }
        else if(mouseOverY == true) {
            state = STATE.CIRCLE_Y;
        }
        else if(mouseOverZ == true) {
            state = STATE.CIRCLE_Z;
        }
        else if(mouseOverPerim == true) {
            state = STATE.CIRCLE_PERIM;
        }
        else {
            state = STATE.TRACKBALL;
            u0 = projectOntoSphere(p);
            u1.zero();
        }

        switch(state) {
            case CIRCLE_X:
            case CIRCLE_Y:
            case CIRCLE_Z:
                pivot = projectOntoPlane(projectOntoSphere(p), 
                        R.mul(basisPerpBasis1[state.ordinal()]),
                        R.mul(basisPerpBasis2[state.ordinal()]));
                pivot.normalizeSelf();
                cursorProj = pivot;
                alpha0 = alpha1 = 0;
                break;
            case CIRCLE_PERIM:
                pivot = p.normalized();
                cursorProj = p.normalized();
                break;
        }
    }

    private Vec3 projectOntoPlane(Vec3 u, Vec3 b1, Vec3 b2) {
        e1 = b1.normalized();
        e2 = b2.normalized();
        return Vec3.add(e1.scale(Vec3.dot(u, e1)), e2.scale(Vec3.dot(u, e2)));
    }

    public void drag(Vec3 p) {
        switch (state) {
            case TRACKBALL:
                u0.set(u1);
                u1 = projectOntoSphere(p);

                v = R.transpose().mul(Vec3.cross(u1, u0));

                Vec3 d = Vec3.sub(u0, u1);
                double t = d.norm() / 2.0;

                t = Util.clamp(t, -1, 1);

                double phi = 2.0 * Math.asin(t);
                q.setFormAxisRadians(v, phi);

                dR = Matrix4x4.mul(q.toMatrix4x4(), dR);
                break;
            case CIRCLE_X:
            case CIRCLE_Y:
            case CIRCLE_Z:
                cursorProj = projectOntoPlane(projectOntoSphere(p),
                        R.mul(basisPerpBasis1[state.ordinal()]),
                        R.mul(basisPerpBasis2[state.ordinal()]));
                cursorProj.normalizeSelf();

                // Compute a plane to determine in which direction the cursor is with respect to the pivot.
                Vec3 signPlaneNormal = Vec3.cross(pivot, R.mul(basis[state.ordinal()]));
                
                alpha0 = alpha1;
                alpha1 = Math.signum(Vec3.dot(signPlaneNormal, cursorProj))*Util.unitangle(pivot, cursorProj);

                q.setFormAxisRadians(basis[state.ordinal()], alpha1);
                
                dR = q.toMatrix4x4();
                break;
            case CIRCLE_PERIM:
                cursorProj = p.normalized();
                Vec3 perpNormal = new Vec3(pivot.v[1], -pivot.v[0], 0.0);
                q.setFormAxisRadians(R.transpose().mul(new Vec3(0,0,1)),
                        Math.signum(Vec3.dot(perpNormal, cursorProj))*Util.unitangle(pivot, cursorProj));
                dR = q.toMatrix4x4();
                break;
            default:
                break;
        }
    }

    public void endDrag(Vec3 p) {
        state = STATE.INACTIVE;
        dR.setIdentity();
    }

    private Vec3 projectOntoSphere(Vec3 p) {
        double d2 = p.norm2();

        if(d2 < radius*radius/2.0) {
            return new Vec3(p.v[0], p.v[1], Math.sqrt(radius2-d2));
        }
        else {
            return new Vec3(p.v[0], p.v[1], 0.5*radius2/Math.sqrt(d2));
        }
    }

    public void render(GL gl) {
        gl.glGetDoublev(GL.GL_PROJECTION_MATRIX, projection.m, 0);

        gl.glEnable(GL.GL_DEPTH_TEST);
        gl.glEnable(GL.GL_POLYGON_OFFSET_FILL);

        gl.glColor3d(1.0, 0, 0);
        gl.glMatrixMode(GL.GL_PROJECTION);
        gl.glPushMatrix();
        gl.glLoadMatrixd(projection.m, 0);

        // Draw the darker middle.

        gl.glPolygonOffset(1, 3);
        gl.glEnable(GL.GL_BLEND);
        gl.glBlendFunc(GL.GL_SRC_ALPHA, GL.GL_ONE_MINUS_SRC_ALPHA);
        
        double alpha = mouseOverZ == false && mouseOverY == false && mouseOverX == false &&
                mouseOver == true ? 0.2 : 0.1;

        gl.glColor4d(0, 0, 0, alpha);

        if(mouseOverPerim == false) {
            Drawing.drawDisk(gl, 0.0, 0.0, radius, segments);
        }
        else {
            Drawing.drawRing(gl, 0, 0, radius, radius*outerCircleRadiusCoeff, segments);
            gl.glColor4d(0, 0, 0, 0);
            Drawing.drawDisk(gl, 0.0, 0.0, radius, segments);

        }

        gl.glDisable(GL.GL_BLEND);

        // Draw perimeters
        gl.glPolygonOffset(1, 2);
        gl.glColor3d(0.0, 0.0, 0.0);
        Drawing.drawCircle(gl, 0.0, 0.0, radius, segments);
        Drawing.drawCircle(gl, 0.0, 0.0, outerCircleRadiusCoeff*radius, segments);


        // Draw the colored orthogonal great circles.
        gl.glPolygonOffset(1, 1);

        gl.glPushMatrix();
        gl.glMultMatrixd(R.m, 0);
        gl.glMultMatrixd(dR.m, 0);
        //gl.glMultMatrixd(R.m, 0);

        // Circle around the Z-axi
        if(state == STATE.CIRCLE_Z) {
            gl.glDisable(GL.GL_DEPTH_TEST);
        }
        gl.glColor3d(0.0, 0.0, 1.0);
        gl.glLineWidth(mouseOverZ == true ? lineWidthHighlight : lineWidth);
        Drawing.drawCircle(gl, 0.0, 0.0, radius, segments);
        gl.glEnable(GL.GL_DEPTH_TEST);

        if(state == STATE.CIRCLE_Y) {
            gl.glDisable(GL.GL_DEPTH_TEST);
        }
        gl.glRotated(90, 1.0, 0.0, 0.0);
        gl.glColor3d(0.0, 1.0, 0.0);
        gl.glLineWidth(mouseOverY == true ? lineWidthHighlight : lineWidth);
        Drawing.drawCircle(gl, 0.0, 0.0, radius, segments);
        gl.glEnable(GL.GL_DEPTH_TEST);

        if(state == STATE.CIRCLE_X) {
            gl.glDisable(GL.GL_DEPTH_TEST);
        }
        gl.glRotated(90, 0.0, 1.0, 0.0);
        gl.glColor3d(1.0, 0.0, 0.0);
        // Circle around the Z-axis
        gl.glLineWidth(mouseOverX == true ? lineWidthHighlight : lineWidth);
        Drawing.drawCircle(gl, 0.0, 0.0, radius, segments);
        gl.glEnable(GL.GL_DEPTH_TEST);


        gl.glPopMatrix(); // Rwork*R

        gl.glDisable(GL.GL_DEPTH_TEST);

        // Draw the perimeter and the outer circle.

        gl.glLineWidth(lineWidth);
        gl.glDisable(GL.GL_DEPTH_TEST);
        gl.glDisable(GL.GL_POLYGON_OFFSET_FILL);

        //gl.glPushMatrix();
        //gl.glMultMatrixd(R.m, 0);
        //gl.glRotated(90, 0.0, 1.0, 0.0);

        //gl.glRotated(thetainit*180.0/Math.PI, 0.0, 1.0, 0.0);

        gl.glPopMatrix();
        gl.glPopMatrix();

        if(state == STATE.CIRCLE_X || state == STATE.CIRCLE_Y || state == STATE.CIRCLE_Z || state == STATE.CIRCLE_PERIM) {
            gl.glColor3dv(stateColor[state.ordinal()].v, 0);
            gl.glLineWidth(lineWidth);

            gl.glPushMatrix();
            gl.glLoadIdentity();
            gl.glLoadMatrixd(projection.m, 0);

            gl.glBegin(GL.GL_LINE_STRIP);
            gl.glVertex3dv(cursorProj.scale(radius).v, 0);
            gl.glVertex3d(0, 0, 0);
            gl.glVertex3dv(pivot.scale(radius).v, 0);
            gl.glEnd();

            gl.glPopMatrix();
        }

        gl.glDisable(GL.GL_DEPTH_TEST);

    }

    public Matrix4x4 getTransform() {
        return dR.transpose();
    }

    public boolean isDragging() {
        return state != STATE.INACTIVE;
    }

    public void setR0(Matrix4x4 R0) {
        this.R = R0;
        dR.setIdentity();
    }

}
